#ifndef U1_SYMMETRIC_MATRIX_H_
#define U1_SYMMETRIC_MATRIX_H_ 
#include <cassert>
#include <vector>

/* Square symmetric matrix.
 * Size of an n-by-n symmetric matrix in memory is n*(n+1)/2 entries times
 * the entry size (sizeof(T)). We effectively store the lower triangle.
 */
template <typename T> class symmetric_matrix
{
  size_t n;
  T *m;

  size_t num_elements(size_t i) const {
    return i*(i+1)/2;
  }

  public:
  symmetric_matrix(size_t n) : n(n) {
    this->m = (T*)calloc(num_elements(n), sizeof(T));
    assert(this->m);
  }

  ~symmetric_matrix() {
    free(m);
  }

  size_t size() const {
    return n;
  }

  const T &operator()(size_t i, size_t j) const {
    if (i < j)
      return (*this)(j, i);
    assert(in_bounds(i,j));
    return m[num_elements(i) + j];
  }

  T &operator()(size_t i, size_t j) {
    if (i < j)
      return (*this)(j, i);
    assert(in_bounds(i,j));
    return m[num_elements(i) + j];
  }

  bool in_bounds(size_t i, size_t j) const {
    return 0 <= i && i < n
        && 0 <= j && j < n;
  }

  void assert_compatible_with(const symmetric_matrix<T> &rhs) const {
#ifndef NDEBUG
    if (this->n != rhs.n) {
      std::cerr<<"Dimension mismatch: "
        <<this->n<<" != "<<rhs.n<<"."<<std::endl;
      exit(EXIT_FAILURE);
    }
#endif
  }

  void operator+=(const symmetric_matrix<T> &rhs) {
    assert_compatible_with(rhs);
    for (size_t i = 0; i < num_elements(n); ++i) {
      this->m[i] += rhs.m[i];
    }
  }

  void operator+=(const T &t) {
    for (size_t i = 0; i < num_elements(n); ++i) {
      this->m[i] += t;
    }
  }

  void operator*=(const symmetric_matrix<T> &rhs) {
    assert_compatible_with(rhs);
    for (size_t i = 0; i < num_elements(n); ++i) {
      this->m[i] *= rhs.m[i];
    }
  }

  void operator*=(const T &t) {
    for (size_t i = 0; i < num_elements(n); ++i) {
      this->m[i] *= t;
    }
  }
  
  void operator/=(const symmetric_matrix<T> &rhs) {
    assert_compatible_with(rhs);
    for (size_t i = 0; i < num_elements(n); ++i) {
      this->m[i] /= rhs.m[i];
    }
  }

  void operator/=(const T &t) {
    for (size_t i = 0; i < num_elements(n); ++i) {
      this->m[i] /= t;
    }
  }

  /**
   * Compute r = Ax, where this is A.
   */
  void left_multiply(const std::vector<T> &x, std::vector<T> &r) const {
    assert(this->size() == x.size());
    r.clear();
    r.resize(x.size(), 0);
    // This could be made more efficient by taking symmetry into account.
    for (size_t i = 0; i < n; ++i) {
      for (size_t j = 0; j < n; ++j) {
        r[i] += (*this)(i,j) * x[j];
      }
    }
  }

  /**
   * Compute r = x^T A x, where this is A.
   */
  T quadratic(const std::vector<T> &x) const {
    T sum = 0;
    for (size_t i = 0; i < n; ++i) {
      for (size_t j = 0; j < n; ++j) {
        sum += (*this)(i,j)*x[i]*x[j];
      }
    }
    return sum;
  }

  void divide_by_if_not_zero(const symmetric_matrix<T> &rhs) {
    assert_compatible_with(rhs);
    for (size_t i = 0; i < num_elements(n); ++i) {
      if (rhs.m[i] != 0)
        this->m[i] /= rhs.m[i];
    }
  }

  void resize(size_t new_n) {
    this->n = new_n;
    this->m = (T*)realloc(this->m, num_elements(n)*sizeof(T));
    assert(this->m);
  }

  /* We assume that our machines all have the same endianness. */
  void read_binary(std::istream &is) {
    for (size_t i = 0; i < num_elements(n); ++i) {
      is.read((char *)&(m[i]), sizeof(T));
    }
  }

  /* We assume that our machines all have the same endianness. */
  void write_binary(std::ostream &os) {
    char buf[sizeof(T)];
    for (size_t i = 0; i < num_elements(n); ++i) {
      memcpy(buf, &(m[i]), sizeof(T));
      os.write(buf, sizeof(T));
    }
  }
};

template<typename T>
std::ostream &operator<<(std::ostream &os, const symmetric_matrix<T> &A) {
  for (size_t i = 0; i < A.size(); ++i) {
    for (size_t j = 0; j < A.size(); ++j) {
      os<<A(i,j)<<' ';
    }
    os<<'\n';
  }
  return os;
}

template<typename T>
void nonnegative_quadratic_opt(const symmetric_matrix<T> &A,
    const std::vector<T> &b, T eps, size_t max_iterations, std::vector<T> &x)
{
  // Initialize x to all ones, to start.
  x.resize(b.size(), 1);

  std::vector<T> r;
  for (size_t it = 0; it < max_iterations; ++it) {
    // residual r = b - A*x;
    A.left_multiply(x, r);
    r *= (T)-1;
    r += b;

    // find active variables - those that are pinned because of
    // nonnegativity constraint, and set respective r_i's to zero
    assert(r.size() == x.size());
    for (size_t i = 0; i < r.size(); ++i) {
      if (fabs(x[i]) < eps && r[i] < 0) {
        r[i] = 0;
      }
    }

    if (L2_norm(r) <= eps)
      return;

    // max step size alpha = (r^T r) / (r^T A r)
    T alpha_den = A.quadratic(r);
    assert(alpha_den != 0);
    T alpha = (r * r) / alpha_den;

    // adjust step size to prevent negative values.
    for (size_t i = 0; i < r.size(); ++i) {
      if (r[i] < 0) {
        alpha = min(alpha, -x[i] / r[i]);
      }
    }

    // x <- x + alpha*r
    r *= alpha;
    x += r;
  }
}

/**
 * A view of an arbitrary subset of the rows (and columns) of a
 * symmetric_matrix. This matrix can be resized by setting a new subindexer.
 *
 * The subindexer must support:
 * size_t size() const = the number of rows (and columns) in the submatrix.
 * size_t operator[size_t i] = the index of row (column) i in the supermatrix.
 *
 * WARNING: not tested
 *
template <typename T, typename subindexer_t = std::vector<size_t> >
class indirect_symmetric_matrix
{
  symmetric_matrix<T> &m;
  subindexer_t pi;
public:
  indirect_symmetric_matrix(symmetric_matrix<T> &m) : m(m) { }
  size_t size() const {
    return this->pi.size();
  }

  T &operator()(size_t i, size_t j) {
    assert(0 <= i && i < this->pi.size());
    assert(0 <= j && j < this->pi.size());
    return this->m(this->pi[i], this->pi[j]);
  }

  const subindexer_t &subindexer() const {
    return this->pi;
  }

  void subindexer(const subindexer_t &pi) {
    this->pi = pi;
  }
};
*/

#endif // guard
/*
* Copyright (c) 2009 John Lees-Miller
* 
* Permission is hereby granted, free of charge, to any person
* obtaining a copy of this software and associated documentation
* files (the "Software"), to deal in the Software without
* restriction, including without limitation the rights to use,
* copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the
* Software is furnished to do so, subject to the following
* conditions:
* 
* The above copyright notice and this permission notice shall be
* included in all copies or substantial portions of the Software.
* 
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
* OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
* OTHER DEALINGS IN THE SOFTWARE.
*/

